/**
 * anis.darwich
 */

package clubhouse.base;

import java.util.Vector;

public class Robot {
	private double X;
	private double Y;
	private double r;
	
	private double[][] sigma;
	
	private Vector<double[]> position;
	private Vector<double[]> estimatePosition;
	private Vector<double[]> errorPosition;
	private Vector<double[]> antislamPosition;
	
	public Robot(double currentX, double currentY, double currentr) {
		this.X = currentX;
		this.Y = currentY;
		this.r = currentr;
		
		this.position = new Vector<double[]>(100,10);
		this.estimatePosition = new Vector<double[]>(100,10);
		this.errorPosition = new Vector<double[]>(100,10);
		this.antislamPosition = new Vector<double[]>(100,10);
		
		this.storePosition();
		
		this.sigma = new double[][]{
			    { 1, 0, 0 },
				{ 0, 1, 0},
			    { 0, 0, 1.0}};
	}
	
	public double getX() {
		return X;
	}
	
	public double getY() {
		return Y;
	}
	
	public double getr() {
		return r;
	}

	public double[][] getSigma() {
		return this.sigma;
	}
	
	public double[] getLastPosition() {
		return position.lastElement();
	}
	
	public void setPosition(double currentX, double currentY, double currentr) {
		this.X = currentX;
		this.Y = currentY;
		this.r = currentr;
	}
	
	public void storePosition() {
		this.position.add(new double[] {X,Y,r});
	}
	
	public void storeEstimatePosition() {
		this.estimatePosition.add(new double[] {X,Y,r});
	}
	
	public void storeErrorPosition() {
		this.errorPosition.add(new double[] {X,Y,r});
	}
	
	public void storeAntislamPosition() {
		this.antislamPosition.add(new double[] {X,Y,r});
	}
	
	public Vector<double[]> getPositions() {
		return this.position;
	}
	
	public Vector<double[]> getEstimatePositions() {
		return this.estimatePosition;
	}
	
	public Vector<double[]> getErrorPositions() {
		return this.errorPosition;
	}
	
	public Vector<double[]> getAntislamPositions() {
		return this.antislamPosition;
	}
	
	public void setNewPosition(double pulseDist, double pulseRotation, int trigo){
		double [] temp = new double[3];
		if (trigo==0) pulseRotation = -pulseRotation;
		
		temp[2] = this.r + pulseRotation*Math.PI/100;
		if (temp[2] <= -Math.PI) temp[2] = temp[2] + 2*Math.PI;
		if (temp[2] > Math.PI) temp[2] = temp[2] - 2*Math.PI;
		temp[0] = this.X + (pulseDist/50)*Math.cos(temp[2]);
		temp[1] = this.Y + (pulseDist/50)*Math.sin(temp[2]);
		
		this.setPosition(temp[0], temp[1], temp[2]);
	}
}
